import rclpy
from rclpy.node import Node

from yyrobot_msg.msg import ArmGoalState
from rclpy.clock import Clock


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(ArmGoalState, 'BlRobotCtrl', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        msg = ArmGoalState()
        time_stamp = Clock().now()
        msg.header.stamp = time_stamp.to_msg()
        msg.cmd = "plan"
        msg.id  = "id"
        msg.goal_type = "J"
        msg.name = ["j1","j2","j3","j4","j5","j6"]
        msg.position = [0.0,0.0,0.1,57.0,0.0,0.0]
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing: "%s"' % msg.cmd)
        self.i += 1

def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
